| Note: This tutorial assumes that you have completed the previous tutorials: Forward kinematics for the PR2 arms. |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Inverse kinematics for the PR2 arms
Description: This tutorial will show you how to use a kinematics node to solve the inverse kinematics and get the joint positions for a desired cartesian position of the PR2 arms.Tutorial Level: INTERMEDIATE
Next Tutorial: Collision free inverse kinematics for the PR2 arms
Contents
In this tutorial, you will learn how to use the PR2 kinematics node to compute inverse kinematics for the tip link of the PR2 arm.
ROS Setup
To use this tutorial make sure that you've run the following:
sudo apt-get install ros-electric-pr2-arm-navigation
Also make sure that in each terminal you use you've run:
export ROBOT=sim
Running in simulation
To use the arm planning stack, we first need to launch the simulator.
roslaunch pr2_gazebo pr2_empty_world.launch
The simulator should come up.
Now, launch the arm navigation stack:
roslaunch pr2_3dnav right_arm_navigation.launch
Next, run this executable from the pr2_arm_navigation_tutorials stack:
rosrun pr2_arm_navigation_tutorials get_ik
If it succeeds, you should see the following output
[ INFO] 0.000000000: Joint: r_shoulder_pan_joint -0.330230 [ INFO] 0.000000000: Joint: r_shoulder_lift_joint 0.008300 [ INFO] 0.000000000: Joint: r_upper_arm_roll_joint -1.550000 [ INFO] 0.000000000: Joint: r_elbow_flex_joint -0.859908 [ INFO] 0.000000000: Joint: r_forearm_roll_joint 3.139403 [ INFO] 0.000000000: Joint: r_wrist_flex_joint -0.529580 [ INFO] 0.000000000: Joint: r_wrist_roll_joint -1.591270
This may fail if the planning scene has not been set. A blank call from the command line will fix this:
rosservice call /environment_server/set_planning_scene_diff '{}'Alternatively, you can set the planning screen from your program that calls the kinematics service:
// Add before main()
#include <arm_navigation_msgs/SetPlanningSceneDiff.h>
static const std::string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";
// Add before you call any kinematics services
// eg. before while (ros::ok()) {}
ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME);
// CHANGE NODEHANDLE 'rh'
ros::ServiceClient set_planning_scene_diff_client = rh.serviceClient<arm_navigation_msgs::SetPlanningSceneDiff>(SET_PLANNING_SCENE_DIFF_NAME);
arm_navigation_msgs::SetPlanningSceneDiff::Request planning_scene_req;
arm_navigation_msgs::SetPlanningSceneDiff::Response planning_scene_res;
if (!set_planning_scene_diff_client.call(planning_scene_req, planning_scene_res)) {
ROS_WARN("Can't set planning scene");
//return 0;
}
The code
The code for this example can be found in the package pr2_arm_navigation_tutorials in the file src/get_ik.cpp , and is valid for the diamondback or electric distributions:
1 #include <ros/ros.h>
2 #include <kinematics_msgs/GetKinematicSolverInfo.h>
3 #include <kinematics_msgs/GetPositionIK.h>
4
5 int main(int argc, char **argv){
6 ros::init (argc, argv, "get_ik");
7 ros::NodeHandle rh;
8
9 ros::service::waitForService("pr2_right_arm_kinematics/get_ik_solver_info");
10 ros::service::waitForService("pr2_right_arm_kinematics/get_ik");
11
12 ros::ServiceClient query_client = rh.serviceClient<kinematics_msgs::GetKinematicSolverInfo>("pr2_right_arm_kinematics/get_ik_solver_info");
13 ros::ServiceClient ik_client = rh.serviceClient<kinematics_msgs::GetPositionIK>("pr2_right_arm_kinematics/get_ik");
14
15 // define the service messages
16 kinematics_msgs::GetKinematicSolverInfo::Request request;
17 kinematics_msgs::GetKinematicSolverInfo::Response response;
18
19 if(query_client.call(request,response))
20 {
21 for(unsigned int i=0; i< response.kinematic_solver_info.joint_names.size(); i++)
22 {
23 ROS_DEBUG("Joint: %d %s",i,response.kinematic_solver_info.joint_names[i].c_str());
24 }
25 }
26 else
27 {
28 ROS_ERROR("Could not call query service");
29 ros::shutdown();
30 exit(1);
31 }
32 // define the service messages
33 kinematics_msgs::GetPositionIK::Request gpik_req;
34 kinematics_msgs::GetPositionIK::Response gpik_res;
35 gpik_req.timeout = ros::Duration(5.0);
36 gpik_req.ik_request.ik_link_name = "r_wrist_roll_link";
37
38 gpik_req.ik_request.pose_stamped.header.frame_id = "torso_lift_link";
39 gpik_req.ik_request.pose_stamped.pose.position.x = 0.75;
40 gpik_req.ik_request.pose_stamped.pose.position.y = -0.188;
41 gpik_req.ik_request.pose_stamped.pose.position.z = 0.0;
42
43 gpik_req.ik_request.pose_stamped.pose.orientation.x = 0.0;
44 gpik_req.ik_request.pose_stamped.pose.orientation.y = 0.0;
45 gpik_req.ik_request.pose_stamped.pose.orientation.z = 0.0;
46 gpik_req.ik_request.pose_stamped.pose.orientation.w = 1.0;
47 gpik_req.ik_request.ik_seed_state.joint_state.position.resize(response.kinematic_solver_info.joint_names.size());
48 gpik_req.ik_request.ik_seed_state.joint_state.name = response.kinematic_solver_info.joint_names;
49 for(unsigned int i=0; i< response.kinematic_solver_info.joint_names.size(); i++)
50 {
51 gpik_req.ik_request.ik_seed_state.joint_state.position[i] = (response.kinematic_solver_info.limits[i].min_position + response.kinematic_solver_info.limits[i].max_position)/2.0;
52 }
53 if(ik_client.call(gpik_req, gpik_res))
54 {
55 if(gpik_res.error_code.val == gpik_res.error_code.SUCCESS)
56 for(unsigned int i=0; i < gpik_res.solution.joint_state.name.size(); i ++)
57 ROS_INFO("Joint: %s %f",gpik_res.solution.joint_state.name[i].c_str(),gpik_res.solution.joint_state.position[i]);
58 else
59 ROS_ERROR("Inverse kinematics failed");
60 }
61 else
62 ROS_ERROR("Inverse kinematics service call failed");
63 ros::shutdown();
64 }
Populating the inverse kinematics service request
The inverse kinematics for the PR2 is (currently) available only for the end-effector links on the two arms - i.e. the r_wrist_roll_link and the l_wrist_roll_link. To populate the inverse kinematics request, you need to fill up three pieces of information:
- A timeout
- The desired pose for the link that you want compute IK for and
A suggested/seed/hint set of positions for the joints used for the IK computation. This set of states will be used to seed the inverse kinematics solver.
Setting a timeout
Setting a timeout is VERY IMPORTANT. If you set a timeout of 0.0, the kinematics node will reject the service request. The inverse kinematics solver is very fast, but we will set a timeout of 5.0 seconds.
35 gpik_req.timeout = ros::Duration(5.0);
Setting the desired pose
We will first set the desired cartesian pose for the link that we want to compute IK for. In this case, its the r_wrist_roll_link of the PR2 robot. We will set a desired cartesian pose for this link in the torso_lift_link frame.
36 gpik_req.ik_request.ik_link_name = "r_wrist_roll_link";
37
38 gpik_req.ik_request.pose_stamped.header.frame_id = "torso_lift_link";
39 gpik_req.ik_request.pose_stamped.pose.position.x = 0.75;
40 gpik_req.ik_request.pose_stamped.pose.position.y = -0.188;
41 gpik_req.ik_request.pose_stamped.pose.position.z = 0.0;
42
43 gpik_req.ik_request.pose_stamped.pose.orientation.x = 0.0;
44 gpik_req.ik_request.pose_stamped.pose.orientation.y = 0.0;
45 gpik_req.ik_request.pose_stamped.pose.orientation.z = 0.0;
46 gpik_req.ik_request.pose_stamped.pose.orientation.w = 1.0;
Seeding the IK solver
The ik_seed_state field is intended for use to seed the IK solver with an initial guess for inverse kinematics. It is important to (at the very least) seed the joints that the kinematics node uses in its computation, i.e. the vector of joints returned by the query service. In this tutorial, we will seed the joint states with values at the middle of the joint limits.
47 gpik_req.ik_request.ik_seed_state.joint_state.position.resize(response.kinematic_solver_info.joint_names.size());
48 gpik_req.ik_request.ik_seed_state.joint_state.name = response.kinematic_solver_info.joint_names;
49 for(unsigned int i=0; i< response.kinematic_solver_info.joint_names.size(); i++)
50 {
51 gpik_req.ik_request.ik_seed_state.joint_state.position[i] = (response.kinematic_solver_info.limits[i].min_position + response.kinematic_solver_info.limits[i].max_position)/2.0;
52 }
Calling the IK service
We can now call the IK service. Note that since we asked for the forward kinematics for two links, the response will contain a vector of poses of size 2.
53 if(ik_client.call(gpik_req, gpik_res))
Interpreting the response
The response to the IK request is contained within a motion_planning_msgs/RobotState message in the service response. For arm inverse kinematics, it is of the form sensor_msgs/JointState. It contains both the joint values for the IK solution and the corresponding joint names.
55 if(gpik_res.error_code.val == gpik_res.error_code.SUCCESS)
56 for(unsigned int i=0; i < gpik_res.solution.joint_state.name.size(); i ++)
57 ROS_INFO("Joint: %s %f",gpik_res.solution.joint_state.name[i].c_str(),gpik_res.solution.joint_state.position[i]);
58 else
59 ROS_ERROR("Inverse kinematics failed");
